/* Planeur */

#include <Wire.h>
#include <Servo.h>
#include "Defines.h"
#include "GPS.h"
#include "Direction.h"
#include "Waypoint.h"

Servo servoDirection; // correspond au servomoteur de direction du planeur
byte registre; // registre du GPS a lire
float data; // donnees lues en provenance du GPS
float k = 20; // coefficient du regulateur
float latitudeWaypoint = 48.418847; // latitude du point GPS a atteindre
float longitudeWaypoint = -4.474217; // longitude du point GPS a atteindre
float capASuivre; // cap a suivre pour atteindre le point GPS, il ne sera calcule qu'une seule fois et initialise dans le setup

/* Fonction de regulation en cap du planeur - capVoulu en degres */
void regulationCap(float capVoulu)
{
  float commande = 0;
  float capSuivi = 0;
  float det = 0;
  float scal = 0;
  int angle = 0;
  
  commande = radians(capVoulu);
  capSuivi = getHeading(); // demande du cap suivi
  
  // Calcul du determinant et du produit scalaire
  det = -(cos(capSuivi)*sin(commande) - cos(commande)*sin(capSuivi));
  scal = cos(capSuivi)*cos(commande) + sin(capSuivi)*sin(commande);
  
  if (scal >= 0) {
    angle = k*det;
  }
  if (scal < 0) {
    if (det < 0) {
      angle = -k;
    }
    else if (det >= 0) {
      angle = k;
    }
  }
  
  // Virage apres calcul de l'angle qui sera envoye en commande au servo
  tourner(angle);
}

/*Fonction permettant au planeur de rejoindre un point GPS */
void regulation(float cap)
{
  float posLat;
  float posLong;
  float dist;
  
  posLat = getLatitude();
  posLong = getLongitude();
  
  // Calcul de la distance au point à atteindre
  dist = distance(posLat, posLong, latitudeWaypoint, longitudeWaypoint);
  dist = 1000*dist; // On convertit la distance en mètres
  
  if(dist > 8)
  {
    regulationCap(cap);
  }
  
  // Cercler à proximité du point
  else 
  {
    tourner(5);
  }
  
}  
  

/* Initialisation */    
void setup()

{
  // Initialisation pour la communication par le bus I2C : Arduino maitre
  Wire.begin();
  Serial.begin(9600);
  
  // Assignement des pins correspondants aux servomoteurs et mise au neutre des servos pendant 1 seconde
  servoDirection.attach(PINSERVODIRECTION);
  servoDirection.write(NEUTRE);
  delay(1000);
  
  // Declaration des variables nécessaires au calcul initial du cap a suivre
  char mode;
  float posLatInit;
  float posLongInit;
  
  mode = getMode();
  
  // Les coordonnees initiales du planeur ne sont pas acquises tant que le GPS n'a pas actualisé sa position et fonctionne en mode autonome
  while (mode != 'A')
  {
    mode = getMode();
    Serial.println("GPS non initialisé.");
  }
  
  // Reception des positions initiales du planeur
  posLatInit = getLatitude();
  posLongInit = getLongitude();
  
  // Calcul du cap a suivre pour rejoindre le point GPS de coordonnees latitudeWaypoint, longitudeWaypoint
  capASuivre = getBearing(posLatInit, posLongInit, latitudeWaypoint, longitudeWaypoint);
  
}

/* Execution du programme en boucle */
void loop()
{

  regulation(capASuivre);
  delay(500);
    
}
